#include "Sys3D.h"
#include "FastOps.h"
#include "Quaternion.h"
#include "Cholesky.h"
#include "LU.h"
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <iomanip>
#include <cmath>

using namespace RSIM;

Sys3D::Sys3D(const int& i,const int& j):barray_(new Body3D[i]),Asm_(NULL),nbodies_(i),
nproc_(j),ids_(new int[j+1]){
        init();
}

Sys3D::~Sys3D(){
        
        delete []barray_;
        delete [] Asm_;
        delete [] ids_;
}                                

void Sys3D::init(){
        
        for(int i=0;i<nbodies_;++i){
                barray_[i].id_ = i;
                barray_[i].init();
        }
        Gravity_[0] = 0.0;
        Gravity_[1] = -9.81;
        Gravity_[2] = 0.0;
        
        pinvjac_.resize(nbodies_,6);
        null_.resize(nbodies_,nbodies_);
        Jac_.resize(6,nbodies_);
}

void Sys3D::collectJac(){
        for(int i=0;i<nbodies_;++i){
                Body3D& bod = barray_[i];
                for(int j=0;j<6;++j)
                        Jac_(j,i) = bod.jac_[j];
        }
}

void Sys3D::n_formDCAHandleEquations_Opt(const int& root,
                                         const int& leaf){
        #ifdef RSIM_SAFE
        assert(leaf >= root);
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif
        
        for(int i=root;i<=leaf;barray_[i++].n_formDCAHandleEquations_Opt(Gravity_));
}

void Sys3D::copyHandleEquationsIntoAssembly(const int& root,
                                            const int& leaf,
                                            const int& asmid,
                                            const bool& copyH2){        
        Asm_[asmid].dca_.copyH1(barray_[root].dca_);        
        Asm_[asmid].parentid_ = root;
        Asm_[asmid].childid_ = leaf;
        
        // no udots associated with a DCA Disassembly of this assembly.
        Asm_[asmid].udotid_ = -1;
        
        if(copyH2)
                Asm_[asmid].dca_.copyH2(barray_[leaf].dca_);
}

void Sys3D::n_sweepForwardLillyOrin_Opt(const int& root,
                                        const int& leaf){
        #ifdef RSIM_SAFE
        assert(leaf >= root);
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif                
        
        barray_[root].n_sweepForwardLillyOrin(NULL);
        for(int i=root+1;i<=leaf;++i)
                barray_[i].n_sweepForwardLillyOrin(barray_+i-1);
                
}

void Sys3D::n_calcDCAHandleEquationsUsingLillyOrin_Opt(const int& root,
                                                       const int& leaf,
                                                       const int& nodeid){
        double *phi22 = Asm_[nodeid].dca_.phi22_.wPtr();
        double *phi23 = Asm_[nodeid].dca_.phi23_.wPtr();
        
        n_sweepForwardLillyOrin_Opt(root,leaf);
        
        Cholesky chol(barray_[leaf].Dat_.Lambda);
        chol.inverse(phi22);
        
//         Matrix I(6,6);
//         I.Identity();
//         LU lu(barray_[leaf].Dat_.Lambda);        
//         Asm_[nodeid].dca_.phi22_ = lu.solve(I);
        
        double t[6];        
        FastOps::mult_mat66_vec6(phi22,barray_[leaf].Dat_.p.Ptr(),t);
        for(int i=0;i<6;++i)
                phi23[i]=-t[i];
}

void Sys3D::n_calcDCAHandleEquationsUsingDCA(   const int& root,
                                                const int& leaf,
                                                const int& asm_start,
                                                const bool& calcH2){        
//         cout<<root<<endl<<leaf<<endl;
        int asm_count = asm_start;
        n_assemble_Opt(barray_[leaf-1],barray_[leaf],Asm_[asm_count],calcH2);
        
        for(int i=leaf-2;i>=root;--i){
//                 cin.get();
//                 barray_[i].dca_.displayH1();
//                 barray_[i].dca_.displayH2();
//                 cin.get();
                n_assemble_Opt(barray_[i],Asm_[asm_count],Asm_[asm_count+1],calcH2);
                ++asm_count;
        }
}

void Sys3D::n_assembleDCAHandleEquationsUsingDCA(){
        if(nproc_>1){
                int count = 0;
                const int& nbpa = nbodies_/nproc_;
                // form nproc_ assemblies as a first step
                for(int i=0;i<nproc_;++i){
                        n_assemble_Opt( barray_[nbpa*i],
                                        barray_[nbpa*i+1],
                                        Asm_[count]);
                        ++count;
                        for(int j=2;j<nbpa;++j){                        
                                n_assemble_Opt(Asm_[count-1],
                                        barray_[nbpa*i+j],
                                        Asm_[count]);
                                ++count;
                        }
                }
                
                // This is the actual Assmebly of the nproc_ assembled systems
                n_assemble_Opt(Asm_[nbpa-2],Asm_[(nbpa-1)*2-1],Asm_[count]);
                ++count;
                for(int i=3;i<=nproc_;++i){
                        n_assemble_Opt(Asm_[count-1],Asm_[(nbpa-1)*(i)-1],Asm_[count]);
                        ++count;
                }            
        }
        else{
                int count = 0;
                n_assemble_Opt(barray_[nbodies_-2],barray_[nbodies_-1],Asm_[0],true);                
                for(int i=nbodies_-3;i>=0;--i){
                        n_assemble_Opt(barray_[i],Asm_[count],Asm_[count+1],true);
                        ++count;
                }
        }
}

void Sys3D::calc_DTXD(const double *I,
                      const double *J,
                      double *m){
        double t[122];
        
        t[2]=I[0]*J[0];
        t[3]=I[12]*J[6];
        t[4]=I[6]*J[3];
        t[5]=t[2]+t[3]+t[4];
        t[6]=I[1]*J[0];
        t[7]=I[13]*J[6];
        t[8]=I[7]*J[3];
        t[9]=t[6]+t[7]+t[8];
        t[10]=I[2]*J[0];
        t[11]=I[14]*J[6];
        t[12]=I[8]*J[3];
        t[13]=t[10]+t[11]+t[12];
        t[14]=I[4]*J[0];
        t[15]=I[10]*J[3];
        t[16]=I[16]*J[6];
        t[17]=t[14]+t[15]+t[16];
        t[18]=I[3]*J[0];
        t[19]=I[15]*J[6];
        t[20]=I[9]*J[3];
        t[21]=t[18]+t[19]+t[20];
        t[22]=I[11]*J[3];
        t[23]=I[5]*J[0];
        t[24]=I[17]*J[6];
        t[25]=t[22]+t[23]+t[24];
        t[26]=I[0]*J[1];
        t[27]=I[12]*J[7];
        t[28]=I[6]*J[4];
        t[29]=t[26]+t[27]+t[28];
        t[30]=I[1]*J[1];
        t[31]=I[13]*J[7];
        t[32]=I[7]*J[4];
        t[33]=t[30]+t[31]+t[32];
        t[34]=I[2]*J[1];
        t[35]=I[14]*J[7];
        t[36]=I[8]*J[4];
        t[37]=t[34]+t[35]+t[36];
        t[38]=I[4]*J[1];
        t[39]=I[10]*J[4];
        t[40]=I[16]*J[7];
        t[41]=t[38]+t[39]+t[40];
        t[42]=I[3]*J[1];
        t[43]=I[15]*J[7];
        t[44]=I[9]*J[4];
        t[45]=t[42]+t[43]+t[44];
        t[46]=I[11]*J[4];
        t[47]=I[5]*J[1];
        t[48]=I[17]*J[7];
        t[49]=t[46]+t[47]+t[48];
        t[50]=I[18]*J[0];
        t[51]=I[24]*J[3];
        t[52]=I[30]*J[6];
        t[53]=t[50]+t[51]+t[52];
        t[54]=I[19]*J[0];
        t[55]=I[25]*J[3];
        t[56]=I[31]*J[6];
        t[57]=t[54]+t[55]+t[56];
        t[58]=I[20]*J[0];
        t[59]=I[26]*J[3];
        t[60]=I[32]*J[6];
        t[61]=t[58]+t[59]+t[60];
        t[62]=I[21]*J[0];
        t[63]=I[27]*J[3];
        t[64]=I[33]*J[6];
        t[65]=t[62]+t[63]+t[64];
        t[66]=I[22]*J[0];
        t[67]=I[28]*J[3];
        t[68]=I[34]*J[6];
        t[69]=t[66]+t[67]+t[68];
        t[70]=I[23]*J[0];
        t[71]=I[29]*J[3];
        t[72]=I[35]*J[6];
        t[73]=t[70]+t[71]+t[72];
        t[74]=I[18]*J[1];
        t[75]=I[24]*J[4];
        t[76]=I[30]*J[7];
        t[77]=t[74]+t[75]+t[76];
        t[78]=I[19]*J[1];
        t[79]=I[25]*J[4];
        t[80]=I[31]*J[7];
        t[81]=t[78]+t[79]+t[80];
        t[82]=I[20]*J[1];
        t[83]=I[26]*J[4];
        t[84]=I[32]*J[7];
        t[85]=t[82]+t[83]+t[84];
        t[86]=I[21]*J[1];
        t[87]=I[27]*J[4];
        t[88]=I[33]*J[7];
        t[89]=t[86]+t[87]+t[88];
        t[90]=I[22]*J[1];
        t[91]=I[28]*J[4];
        t[92]=I[34]*J[7];
        t[93]=t[90]+t[91]+t[92];
        t[94]=I[23]*J[1];
        t[95]=I[29]*J[4];
        t[96]=I[35]*J[7];
        t[97]=t[94]+t[95]+t[96];
        t[98]=I[18]*J[2];
        t[99]=I[24]*J[5];
        t[100]=I[30]*J[8];
        t[101]=t[100]+t[98]+t[99];
        t[102]=I[19]*J[2];
        t[103]=I[25]*J[5];
        t[104]=I[31]*J[8];
        t[105]=t[102]+t[103]+t[104];
        t[106]=I[20]*J[2];
        t[107]=I[26]*J[5];
        t[108]=I[32]*J[8];
        t[109]=t[106]+t[107]+t[108];
        t[110]=I[21]*J[2];
        t[111]=I[27]*J[5];
        t[112]=I[33]*J[8];
        t[113]=t[110]+t[111]+t[112];
        t[114]=I[22]*J[2];
        t[115]=I[28]*J[5];
        t[116]=I[34]*J[8];
        t[117]=t[114]+t[115]+t[116];
        t[118]=I[23]*J[2];
        t[119]=I[29]*J[5];
        t[120]=I[35]*J[8];
        t[121]=t[118]+t[119]+t[120];
        
        m[0]=J[0]*t[5]+J[6]*t[13]+J[3]*t[9];
        m[1]=J[1]*t[5]+J[7]*t[13]+J[4]*t[9];
        m[2]=J[0]*t[21]+J[3]*t[17]+J[6]*t[25];
        m[3]=J[1]*t[21]+J[4]*t[17]+J[7]*t[25];
        m[4]=J[2]*t[21]+J[5]*t[17]+J[8]*t[25];
        m[5]=J[0]*t[29]+J[3]*t[33]+J[6]*t[37];
        m[6]=J[1]*t[29]+J[4]*t[33]+J[7]*t[37];
        m[7]=J[0]*t[45]+J[3]*t[41]+J[6]*t[49];
        m[8]=J[1]*t[45]+J[4]*t[41]+J[7]*t[49];
        m[9]=J[2]*t[45]+J[5]*t[41]+J[8]*t[49];
        m[10]=J[0]*t[53]+J[3]*t[57]+J[6]*t[61];
        m[11]=J[1]*t[53]+J[4]*t[57]+J[7]*t[61];
        m[12]=J[0]*t[65]+J[3]*t[69]+J[6]*t[73];
        m[13]=J[1]*t[65]+J[4]*t[69]+J[7]*t[73];
        m[14]=J[2]*t[65]+J[5]*t[69]+J[8]*t[73];
        m[15]=J[0]*t[77]+J[3]*t[81]+J[6]*t[85];
        m[16]=J[1]*t[77]+J[4]*t[81]+J[7]*t[85];
        m[17]=J[0]*t[89]+J[3]*t[93]+J[6]*t[97];
        m[18]=J[1]*t[89]+J[4]*t[93]+J[7]*t[97];
        m[19]=J[2]*t[89]+J[5]*t[93]+J[8]*t[97];
        m[20]=J[0]*t[101]+J[3]*t[105]+J[6]*t[109];
        m[21]=J[1]*t[101]+J[4]*t[105]+J[7]*t[109];
        m[22]=J[0]*t[113]+J[3]*t[117]+J[6]*t[121];
        m[23]=J[1]*t[113]+J[4]*t[117]+J[7]*t[121];
        m[24]=J[2]*t[113]+J[5]*t[117]+J[8]*t[121];
}

void Sys3D::calc_xtilde(const double *I,
                        const double *J,
                        double *op){
        double t[122],m[25];
        
        t[2]=I[0]*J[0];
        t[3]=I[12]*J[6];
        t[4]=I[6]*J[3];
        t[5]=t[2]+t[3]+t[4];
        t[6]=I[1]*J[0];
        t[7]=I[13]*J[6];
        t[8]=I[7]*J[3];
        t[9]=t[6]+t[7]+t[8];
        t[10]=I[2]*J[0];
        t[11]=I[14]*J[6];
        t[12]=I[8]*J[3];
        t[13]=t[10]+t[11]+t[12];
        t[14]=I[4]*J[0];
        t[15]=I[10]*J[3];
        t[16]=I[16]*J[6];
        t[17]=t[14]+t[15]+t[16];
        t[18]=I[3]*J[0];
        t[19]=I[15]*J[6];
        t[20]=I[9]*J[3];
        t[21]=t[18]+t[19]+t[20];
        t[22]=I[11]*J[3];
        t[23]=I[5]*J[0];
        t[24]=I[17]*J[6];
        t[25]=t[22]+t[23]+t[24];
        t[26]=I[0]*J[1];
        t[27]=I[12]*J[7];
        t[28]=I[6]*J[4];
        t[29]=t[26]+t[27]+t[28];
        t[30]=I[1]*J[1];
        t[31]=I[13]*J[7];
        t[32]=I[7]*J[4];
        t[33]=t[30]+t[31]+t[32];
        t[34]=I[2]*J[1];
        t[35]=I[14]*J[7];
        t[36]=I[8]*J[4];
        t[37]=t[34]+t[35]+t[36];
        t[38]=I[4]*J[1];
        t[39]=I[10]*J[4];
        t[40]=I[16]*J[7];
        t[41]=t[38]+t[39]+t[40];
        t[42]=I[3]*J[1];
        t[43]=I[15]*J[7];
        t[44]=I[9]*J[4];
        t[45]=t[42]+t[43]+t[44];
        t[46]=I[11]*J[4];
        t[47]=I[5]*J[1];
        t[48]=I[17]*J[7];
        t[49]=t[46]+t[47]+t[48];
        t[50]=I[18]*J[0];
        t[51]=I[24]*J[3];
        t[52]=I[30]*J[6];
        t[53]=t[50]+t[51]+t[52];
        t[54]=I[19]*J[0];
        t[55]=I[25]*J[3];
        t[56]=I[31]*J[6];
        t[57]=t[54]+t[55]+t[56];
        t[58]=I[20]*J[0];
        t[59]=I[26]*J[3];
        t[60]=I[32]*J[6];
        t[61]=t[58]+t[59]+t[60];
        t[62]=I[21]*J[0];
        t[63]=I[27]*J[3];
        t[64]=I[33]*J[6];
        t[65]=t[62]+t[63]+t[64];
        t[66]=I[22]*J[0];
        t[67]=I[28]*J[3];
        t[68]=I[34]*J[6];
        t[69]=t[66]+t[67]+t[68];
        t[70]=I[23]*J[0];
        t[71]=I[29]*J[3];
        t[72]=I[35]*J[6];
        t[73]=t[70]+t[71]+t[72];
        t[74]=I[18]*J[1];
        t[75]=I[24]*J[4];
        t[76]=I[30]*J[7];
        t[77]=t[74]+t[75]+t[76];
        t[78]=I[19]*J[1];
        t[79]=I[25]*J[4];
        t[80]=I[31]*J[7];
        t[81]=t[78]+t[79]+t[80];
        t[82]=I[20]*J[1];
        t[83]=I[26]*J[4];
        t[84]=I[32]*J[7];
        t[85]=t[82]+t[83]+t[84];
        t[86]=I[21]*J[1];
        t[87]=I[27]*J[4];
        t[88]=I[33]*J[7];
        t[89]=t[86]+t[87]+t[88];
        t[90]=I[22]*J[1];
        t[91]=I[28]*J[4];
        t[92]=I[34]*J[7];
        t[93]=t[90]+t[91]+t[92];
        t[94]=I[23]*J[1];
        t[95]=I[29]*J[4];
        t[96]=I[35]*J[7];
        t[97]=t[94]+t[95]+t[96];
        t[98]=I[18]*J[2];
        t[99]=I[24]*J[5];
        t[100]=I[30]*J[8];
        t[101]=t[100]+t[98]+t[99];
        t[102]=I[19]*J[2];
        t[103]=I[25]*J[5];
        t[104]=I[31]*J[8];
        t[105]=t[102]+t[103]+t[104];
        t[106]=I[20]*J[2];
        t[107]=I[26]*J[5];
        t[108]=I[32]*J[8];
        t[109]=t[106]+t[107]+t[108];
        t[110]=I[21]*J[2];
        t[111]=I[27]*J[5];
        t[112]=I[33]*J[8];
        t[113]=t[110]+t[111]+t[112];
        t[114]=I[22]*J[2];
        t[115]=I[28]*J[5];
        t[116]=I[34]*J[8];
        t[117]=t[114]+t[115]+t[116];
        t[118]=I[23]*J[2];
        t[119]=I[29]*J[5];
        t[120]=I[35]*J[8];
        t[121]=t[118]+t[119]+t[120];
        
        m[0]=J[0]*t[5]+J[6]*t[13]+J[3]*t[9];
        m[1]=J[1]*t[5]+J[7]*t[13]+J[4]*t[9];
        m[2]=J[0]*t[21]+J[3]*t[17]+J[6]*t[25];
        m[3]=J[1]*t[21]+J[4]*t[17]+J[7]*t[25];
        m[4]=J[2]*t[21]+J[5]*t[17]+J[8]*t[25];
        m[5]=J[0]*t[29]+J[3]*t[33]+J[6]*t[37];
        m[6]=J[1]*t[29]+J[4]*t[33]+J[7]*t[37];
        m[7]=J[0]*t[45]+J[3]*t[41]+J[6]*t[49];
        m[8]=J[1]*t[45]+J[4]*t[41]+J[7]*t[49];
        m[9]=J[2]*t[45]+J[5]*t[41]+J[8]*t[49];
        m[10]=J[0]*t[53]+J[3]*t[57]+J[6]*t[61];
        m[11]=J[1]*t[53]+J[4]*t[57]+J[7]*t[61];
        m[12]=J[0]*t[65]+J[3]*t[69]+J[6]*t[73];
        m[13]=J[1]*t[65]+J[4]*t[69]+J[7]*t[73];
        m[14]=J[2]*t[65]+J[5]*t[69]+J[8]*t[73];
        m[15]=J[0]*t[77]+J[3]*t[81]+J[6]*t[85];
        m[16]=J[1]*t[77]+J[4]*t[81]+J[7]*t[85];
        m[17]=J[0]*t[89]+J[3]*t[93]+J[6]*t[97];
        m[18]=J[1]*t[89]+J[4]*t[93]+J[7]*t[97];
        m[19]=J[2]*t[89]+J[5]*t[93]+J[8]*t[97];
        m[20]=J[0]*t[101]+J[3]*t[105]+J[6]*t[109];
        m[21]=J[1]*t[101]+J[4]*t[105]+J[7]*t[109];
        m[22]=J[0]*t[113]+J[3]*t[117]+J[6]*t[121];
        m[23]=J[1]*t[113]+J[4]*t[117]+J[7]*t[121];
        m[24]=J[2]*t[113]+J[5]*t[117]+J[8]*t[121];        
        
        Cholesky chol(Matrix(5,5,m));        
        chol.inverse(op);        
}

void Sys3D::calc_DXDT(const double *I,
                     const double *J,
                     double *m){
        double t[107];
        
        t[2]=I[0]*J[0];
        t[3]=I[5]*J[1];
        t[4]=t[2]+t[3];
        t[5]=I[1]*J[0];
        t[6]=I[6]*J[1];
        t[7]=t[5]+t[6];
        t[8]=I[2]*J[0];
        t[9]=I[7]*J[1];
        t[10]=t[8]+t[9];
        t[11]=I[3]*J[0];
        t[12]=I[8]*J[1];
        t[13]=t[11]+t[12];
        t[14]=I[4]*J[0];
        t[15]=I[9]*J[1];
        t[16]=t[14]+t[15];
        t[17]=I[0]*J[3];
        t[18]=I[5]*J[4];
        t[19]=t[17]+t[18];
        t[20]=I[1]*J[3];
        t[21]=I[6]*J[4];
        t[22]=t[20]+t[21];
        t[23]=I[2]*J[3];
        t[24]=I[7]*J[4];
        t[25]=t[23]+t[24];
        t[26]=I[3]*J[3];
        t[27]=I[8]*J[4];
        t[28]=t[26]+t[27];
        t[29]=I[4]*J[3];
        t[30]=I[9]*J[4];
        t[31]=t[29]+t[30];
        t[32]=I[0]*J[6];
        t[33]=I[5]*J[7];
        t[34]=t[32]+t[33];
        t[35]=I[1]*J[6];
        t[36]=I[6]*J[7];
        t[37]=t[35]+t[36];
        t[38]=I[2]*J[6];
        t[39]=I[7]*J[7];
        t[40]=t[38]+t[39];
        t[41]=I[3]*J[6];
        t[42]=I[8]*J[7];
        t[43]=t[41]+t[42];
        t[44]=I[4]*J[6];
        t[45]=I[9]*J[7];
        t[46]=t[44]+t[45];
        t[47]=I[10]*J[0];
        t[48]=I[15]*J[1];
        t[49]=I[20]*J[2];
        t[50]=t[47]+t[48]+t[49];
        t[51]=I[11]*J[0];
        t[52]=I[16]*J[1];
        t[53]=I[21]*J[2];
        t[54]=t[51]+t[52]+t[53];
        t[55]=I[12]*J[0];
        t[56]=I[17]*J[1];
        t[57]=I[22]*J[2];
        t[58]=t[55]+t[56]+t[57];
        t[59]=I[13]*J[0];
        t[60]=I[18]*J[1];
        t[61]=I[23]*J[2];
        t[62]=t[59]+t[60]+t[61];
        t[63]=I[14]*J[0];
        t[64]=I[19]*J[1];
        t[65]=I[24]*J[2];
        t[66]=t[63]+t[64]+t[65];
        t[67]=I[10]*J[3];
        t[68]=I[15]*J[4];
        t[69]=I[20]*J[5];
        t[70]=t[67]+t[68]+t[69];
        t[71]=I[11]*J[3];
        t[72]=I[16]*J[4];
        t[73]=I[21]*J[5];
        t[74]=t[71]+t[72]+t[73];
        t[75]=I[12]*J[3];
        t[76]=I[17]*J[4];
        t[77]=I[22]*J[5];
        t[78]=t[75]+t[76]+t[77];
        t[79]=I[13]*J[3];
        t[80]=I[18]*J[4];
        t[81]=I[23]*J[5];
        t[82]=t[79]+t[80]+t[81];
        t[83]=I[14]*J[3];
        t[84]=I[19]*J[4];
        t[85]=I[24]*J[5];
        t[86]=t[83]+t[84]+t[85];
        t[87]=I[10]*J[6];
        t[88]=I[15]*J[7];
        t[89]=I[20]*J[8];
        t[90]=t[87]+t[88]+t[89];
        t[91]=I[11]*J[6];
        t[92]=I[16]*J[7];
        t[93]=I[21]*J[8];
        t[94]=t[91]+t[92]+t[93];
        t[95]=I[12]*J[6];
        t[96]=I[17]*J[7];
        t[97]=I[22]*J[8];
        t[98]=t[95]+t[96]+t[97];
        t[99]=I[13]*J[6];
        t[100]=I[18]*J[7];
        t[101]=I[23]*J[8];
        t[102]=t[100]+t[101]+t[99];
        t[103]=I[14]*J[6];
        t[104]=I[19]*J[7];
        t[105]=I[24]*J[8];
        t[106]=t[103]+t[104]+t[105];
        
        m[0]=J[0]*t[4]+J[1]*t[7];
        m[1]=J[3]*t[4]+J[4]*t[7];
        m[2]=J[6]*t[4]+J[7]*t[7];
        m[3]=J[0]*t[10]+J[1]*t[13]+J[2]*t[16];
        m[4]=J[3]*t[10]+J[4]*t[13]+J[5]*t[16];
        m[5]=J[6]*t[10]+J[7]*t[13]+J[8]*t[16];
        m[6]=J[0]*t[19]+J[1]*t[22];
        m[7]=J[3]*t[19]+J[4]*t[22];
        m[8]=J[6]*t[19]+J[7]*t[22];
        m[9]=J[0]*t[25]+J[1]*t[28]+J[2]*t[31];
        m[10]=J[3]*t[25]+J[4]*t[28]+J[5]*t[31];
        m[11]=J[6]*t[25]+J[7]*t[28]+J[8]*t[31];
        m[12]=J[0]*t[34]+J[1]*t[37];
        m[13]=J[3]*t[34]+J[4]*t[37];
        m[14]=J[6]*t[34]+J[7]*t[37];
        m[15]=J[0]*t[40]+J[1]*t[43]+J[2]*t[46];
        m[16]=J[3]*t[40]+J[4]*t[43]+J[5]*t[46];
        m[17]=J[6]*t[40]+J[7]*t[43]+J[8]*t[46];
        m[18]=J[0]*t[50]+J[1]*t[54];
        m[19]=J[3]*t[50]+J[4]*t[54];
        m[20]=J[6]*t[50]+J[7]*t[54];
        m[21]=J[0]*t[58]+J[1]*t[62]+J[2]*t[66];
        m[22]=J[3]*t[58]+J[4]*t[62]+J[5]*t[66];
        m[23]=J[6]*t[58]+J[7]*t[62]+J[8]*t[66];
        m[24]=J[0]*t[70]+J[1]*t[74];
        m[25]=J[3]*t[70]+J[4]*t[74];
        m[26]=J[6]*t[70]+J[7]*t[74];
        m[27]=J[0]*t[78]+J[1]*t[82]+J[2]*t[86];
        m[28]=J[3]*t[78]+J[4]*t[82]+J[5]*t[86];
        m[29]=J[6]*t[78]+J[7]*t[82]+J[8]*t[86];
        m[30]=J[0]*t[90]+J[1]*t[94];
        m[31]=J[3]*t[90]+J[4]*t[94];
        m[32]=J[6]*t[90]+J[7]*t[94];
        m[33]=J[1]*t[102]+J[2]*t[106]+J[0]*t[98];
        m[34]=J[4]*t[102]+J[5]*t[106]+J[3]*t[98];
        m[35]=J[7]*t[102]+J[8]*t[106]+J[6]*t[98];
}

void Sys3D::n_assemble_Opt(const Body3D& parent, 
                           const Body3D& child,
                           DCAAssembly3D& A,
                           const bool& calcH2,
                           const bool& calcH1){
        
        A.parentid_ = parent.id_;
        A.childid_ = child.id_;
        A.udotid_ = A.childid_;
        A.parentasmid_ = -1;
        A.childasmid_ = -1;
        
        const DCAData3D& p = parent.dca_;
        const DCAData3D& c = child.dca_;
        
        const Vect3& p_w = parent.w_;
        const Vect3& c_w = child.w_;
        const double *J = parent.R_NM_.Ptr();
        
        const double *p_phi11 = p.phi11_.Ptr();
        const double *p_phi12 = p.phi12_.Ptr();
        const double *p_phi13 = p.phi13_.Ptr();
        
        const double *p_phi21 = p.phi21_.Ptr();
        const double *p_phi22 = p.phi22_.Ptr();
        const double *p_phi23 = p.phi23_.Ptr();
        
        const double *c_phi11 = c.phi11_.Ptr();
        const double *c_phi12 = c.phi12_.Ptr();
        const double *c_phi13 = c.phi13_.Ptr();
        
        const double *c_phi21 = c.phi21_.Ptr();
        const double *c_phi22 = c.phi22_.Ptr();
        const double *c_phi23 = c.phi23_.Ptr();
        
        // t(0,35) = D*inv{(~D)*(c.phi11_ + p.phi22_)*D}*(~D)
        double T[36],z[25],t[36];
        for(int i=0;i<36;++i)
                T[i]=c_phi11[i]+p_phi22[i];
        
        calc_xtilde(T,J,z);
        calc_DXDT(z,J,t);
                
        double *wa = A.Wa_.wPtr();                 
        double *wb = A.Wb_.wPtr();                 
        double *wc = A.Wc_.wPtr();      
            
        if(calcH1){
                // calculate Wa
                // A.Wa_ = X*p.phi21_;                         
                FastOps::mult_mat66_mat66(t,p_phi21,wa);
        }
                        
        if(calcH2){
                // calculate Wb
                // A.Wb_ = -X*c.phi12_;                                
                FastOps::mmult_mat66_mat66(t,c_phi12,wb);
        }        
        {
                double w[3],z[6];
                FastOps::cross(p_w.Ptr(),c_w.Ptr(),w);
                // calculate Wc
                // A.Wc_ = Vect6(X*(p.phi23_ - c.phi13_+ dot(H)u));        
                for(int i=0;i<6;++i)
                        z[i]=p_phi23[i]-c_phi13[i];
                z[0]+=w[0];
                z[1]+=w[1];
                z[2]+=w[2];
                FastOps::mult_mat66_vec6(t,z,wc);                
        }
        double m[36];
        if(calcH1){
                // calculate A.dca_.phi11_
                // A.dca_.phi11_ = p.phi11_ - p.phi12_*A.Wa_;
                double *phi11 = A.dca_.phi11_.wPtr();                
                FastOps::mult_mat66_mat66(p_phi12,wa,m);
                for(int i=0;i<36;++i)
                        phi11[i] = p_phi11[i]-m[i];                                
                if(calcH2){
                        // calculate A.dca_.phi12_
                        // A.dca_.phi12_ = -p.phi12_*A.Wb_;
                        double *phi12 = A.dca_.phi12_.wPtr();                
                        FastOps::mmult_mat66_mat66(p_phi12,wb,phi12);                
                }
                        
                {
                        // calculate A.dca_.phi13_
                        // A.dca_.phi13_ = p.phi13_ - p.phi12_*A.Wc_
                        double *phi13 = A.dca_.phi13_.wPtr();
                        FastOps::mult_mat66_vec6(p_phi12,wc,m);
                        for(int i=0;i<6;++i)
                                phi13[i] = p_phi13[i]-m[i];                
                }
        }
        // Skip handle 2 calculations
        if(calcH2){
                if(calcH1){
                        // calculate A.dca_.phi21_
                        // A.dca_.phi21_ = c.phi21_*A.Wa_;
                        double *phi21 = A.dca_.phi21_.wPtr();
                        FastOps::mult_mat66_mat66(c_phi21,wa,phi21);
                }
                {
                        // calculate A.dca_.phi22_
                        // A.dca_.phi22_ = c.phi22_ + c.phi21_*A.Wb_;
                        double *phi22 = A.dca_.phi22_.wPtr();
                        FastOps::mult_mat66_mat66(c_phi21,wb,m);
                        for(int i=0;i<36;++i)
                                phi22[i]=c_phi22[i]+m[i];                
                }
                {
                        // calculate A.dca_.phi23_
                        // calculate A.dca_.phi23_ = c.phi23_ + c.phi21_*A.Wc_;
                        double *phi23 = A.dca_.phi23_.wPtr();
                        FastOps::mult_mat66_vec6(c_phi21,wc,m);
                        for(int i=0;i<6;++i)
                                phi23[i] = c_phi23[i] + m[i];
                }          
        }
}

void Sys3D::n_assemble_Opt(const DCAAssembly3D& parent, 
                           const DCAAssembly3D& child,
                           DCAAssembly3D& A,
                           const bool& calcH2,
                           const bool& calcH1){
        
        A.parentid_ = parent.parentid_;
        A.childid_ = child.childid_;
        A.udotid_ = child.parentid_;
        A.parentasmid_ = parent.asmid_;
        A.childasmid_ = child.asmid_;        
        
        const DCAData3D& p = parent.dca_;
        const DCAData3D& c = child.dca_;        
                
        const Body3D& parentbody = barray_[child.parentid_-1];
        const Body3D& childbody = barray_[child.parentid_];
        
        const Vect3& p_w = parentbody.w_;
        const Vect3& c_w = childbody.w_;
        const double *J = parentbody.R_NM_.Ptr();
        
        const double *p_phi11 = p.phi11_.Ptr();
        const double *p_phi12 = p.phi12_.Ptr();
        const double *p_phi13 = p.phi13_.Ptr();
        
        const double *p_phi21 = p.phi21_.Ptr();
        const double *p_phi22 = p.phi22_.Ptr();
        const double *p_phi23 = p.phi23_.Ptr();
        
        const double *c_phi11 = c.phi11_.Ptr();
        const double *c_phi12 = c.phi12_.Ptr();
        const double *c_phi13 = c.phi13_.Ptr();
        
        const double *c_phi21 = c.phi21_.Ptr();
        const double *c_phi22 = c.phi22_.Ptr();
        const double *c_phi23 = c.phi23_.Ptr();
        
        // t(0,35) = D*inv{(~D)*(c.phi11_ + p.phi22_)*D}*(~D)
        double T[36],z[25],t[36];
        for(int i=0;i<36;++i)
                T[i]=c_phi11[i]+p_phi22[i];
        
        calc_xtilde(T,J,z);
        calc_DXDT(z,J,t);
        
        double *wa = A.Wa_.wPtr();                 
        double *wb = A.Wb_.wPtr();                 
        double *wc = A.Wc_.wPtr();      
        
        if(calcH1){
                // calculate Wa
                // A.Wa_ = X*p.phi21_;                         
                FastOps::mult_mat66_mat66(t,p_phi21,wa);
        }
        
        if(calcH2){
                // calculate Wb
                // A.Wb_ = -X*c.phi12_;                                
                FastOps::mmult_mat66_mat66(t,c_phi12,wb);
        }        
        {
                double w[3],z[6];
                FastOps::cross(p_w.Ptr(),c_w.Ptr(),w);
                // calculate Wc
                // A.Wc_ = Vect6(X*(p.phi23_ - c.phi13_+ dot(H)u));        
                for(int i=0;i<6;++i)
                        z[i]=p_phi23[i]-c_phi13[i];
                z[0]+=w[0];
                z[1]+=w[1];
                z[2]+=w[2];
                FastOps::mult_mat66_vec6(t,z,wc);                
        }
        double m[36];
        
        if(calcH1){
                // calculate A.dca_.phi11_
                // A.dca_.phi11_ = p.phi11_ - p.phi12_*A.Wa_;
                double *phi11 = A.dca_.phi11_.wPtr();                
                FastOps::mult_mat66_mat66(p_phi12,wa,m);
                for(int i=0;i<36;++i)
                        phi11[i] = p_phi11[i]-m[i];                
        
                if(calcH2){
                        // calculate A.dca_.phi12_
                        // A.dca_.phi12_ = -p.phi12_*A.Wb_;
                        double *phi12 = A.dca_.phi12_.wPtr();                
                        FastOps::mmult_mat66_mat66(p_phi12,wb,phi12);                
                }
                
                {
                        // calculate A.dca_.phi13_
                        // A.dca_.phi13_ = p.phi13_ - p.phi12_*A.Wc_
                        double *phi13 = A.dca_.phi13_.wPtr();
                        FastOps::mult_mat66_vec6(p_phi12,wc,m);
                        for(int i=0;i<6;++i)
                                phi13[i] = p_phi13[i]-m[i];                
                }
        }
        if(calcH2){
                if(calcH1){
                        // calculate A.dca_.phi21_
                        // A.dca_.phi21_ = c.phi21_*A.Wa_;
                        double *phi21 = A.dca_.phi21_.wPtr();
                        FastOps::mult_mat66_mat66(c_phi21,wa,phi21);
                }
                {
                        // calculate A.dca_.phi22_
                        // A.dca_.phi22_ = c.phi22_ + c.phi21_*A.Wb_;
                        double *phi22 = A.dca_.phi22_.wPtr();
                        FastOps::mult_mat66_mat66(c_phi21,wb,m);
                        for(int i=0;i<36;++i)
                                phi22[i]=c_phi22[i]+m[i];                
                }
                {
                        // calculate A.dca_.phi23_
                        // calculate A.dca_.phi23_ = c.phi23_ + c.phi21_*A.Wc_;
                        double *phi23 = A.dca_.phi23_.wPtr();
                        FastOps::mult_mat66_vec6(c_phi21,wc,m);
                        for(int i=0;i<6;++i)
                                phi23[i] = c_phi23[i] + m[i];
                }          
        }
}

void Sys3D::n_assemble_Opt(const DCAAssembly3D& parent, 
                           const Body3D& child,
                           DCAAssembly3D& A,
                           const bool& calcH2,
                           const bool& calcH1){
        A.parentid_ = parent.parentid_;
        A.childid_ = child.id_;
        A.udotid_ = child.id_;
        A.parentasmid_ = parent.asmid_;        
        A.childasmid_ = -1;
        
        const DCAData3D& p = parent.dca_;
        const DCAData3D& c = child.dca_;
        
        const Body3D& parentbody = barray_[child.id_-1];
        const Body3D& childbody = child;
        
        const Vect3& p_w = parentbody.w_;
        const Vect3& c_w = childbody.w_;
        const double *J = parentbody.R_NM_.Ptr();
        
        const double *p_phi11 = p.phi11_.Ptr();
        const double *p_phi12 = p.phi12_.Ptr();
        const double *p_phi13 = p.phi13_.Ptr();
        
        const double *p_phi21 = p.phi21_.Ptr();
        const double *p_phi22 = p.phi22_.Ptr();
        const double *p_phi23 = p.phi23_.Ptr();
        
        const double *c_phi11 = c.phi11_.Ptr();
        const double *c_phi12 = c.phi12_.Ptr();
        const double *c_phi13 = c.phi13_.Ptr();
        
        const double *c_phi21 = c.phi21_.Ptr();
        const double *c_phi22 = c.phi22_.Ptr();
        const double *c_phi23 = c.phi23_.Ptr();
        
        // t(0,35) = D*inv{(~D)*(c.phi11_ + p.phi22_)*D}*(~D)
        double T[36],z[25],t[36];
        for(int i=0;i<36;++i)
                T[i]=c_phi11[i]+p_phi22[i];
        
        calc_xtilde(T,J,z);
        calc_DXDT(z,J,t);
        
        double *wa = A.Wa_.wPtr();                 
        double *wb = A.Wb_.wPtr();                 
        double *wc = A.Wc_.wPtr();      
        
        if(calcH1){
                // calculate Wa
                // A.Wa_ = X*p.phi21_;                         
                FastOps::mult_mat66_mat66(t,p_phi21,wa);
        }
        
        if(calcH2){
                // calculate Wb
                // A.Wb_ = -X*c.phi12_;                                
                FastOps::mmult_mat66_mat66(t,c_phi12,wb);
        }        
        {
                double w[3],z[6];
                FastOps::cross(p_w.Ptr(),c_w.Ptr(),w);
                // calculate Wc
                // A.Wc_ = Vect6(X*(p.phi23_ - c.phi13_+ dot(H)u));        
                for(int i=0;i<6;++i)
                        z[i]=p_phi23[i]-c_phi13[i];
                z[0]+=w[0];
                z[1]+=w[1];
                z[2]+=w[2];
                FastOps::mult_mat66_vec6(t,z,wc);                
        }
        double m[36];
        if(calcH1){
                // calculate A.dca_.phi11_
                // A.dca_.phi11_ = p.phi11_ - p.phi12_*A.Wa_;
                double *phi11 = A.dca_.phi11_.wPtr();                
                FastOps::mult_mat66_mat66(p_phi12,wa,m);
                for(int i=0;i<36;++i)
                        phi11[i] = p_phi11[i]-m[i];                
        
                if(calcH2){
                        // calculate A.dca_.phi12_
                        // A.dca_.phi12_ = -p.phi12_*A.Wb_;
                        double *phi12 = A.dca_.phi12_.wPtr();                
                        FastOps::mmult_mat66_mat66(p_phi12,wb,phi12);                
                }
                
                {
                        // calculate A.dca_.phi13_
                        // A.dca_.phi13_ = p.phi13_ - p.phi12_*A.Wc_
                        double *phi13 = A.dca_.phi13_.wPtr();
                        FastOps::mult_mat66_vec6(p_phi12,wc,m);
                        for(int i=0;i<6;++i)
                                phi13[i] = p_phi13[i]-m[i];                
                }
        }
        if(calcH2){
                if(calcH1){
                        // calculate A.dca_.phi21_
                        // A.dca_.phi21_ = c.phi21_*A.Wa_;
                        double *phi21 = A.dca_.phi21_.wPtr();
                        FastOps::mult_mat66_mat66(c_phi21,wa,phi21);
                }
                {
                        // calculate A.dca_.phi22_
                        // A.dca_.phi22_ = c.phi22_ + c.phi21_*A.Wb_;
                        double *phi22 = A.dca_.phi22_.wPtr();
                        FastOps::mult_mat66_mat66(c_phi21,wb,m);
                        for(int i=0;i<36;++i)
                                phi22[i]=c_phi22[i]+m[i];                
                }
                {
                        // calculate A.dca_.phi23_
                        // calculate A.dca_.phi23_ = c.phi23_ + c.phi21_*A.Wc_;
                        double *phi23 = A.dca_.phi23_.wPtr();
                        FastOps::mult_mat66_vec6(c_phi21,wc,m);
                        for(int i=0;i<6;++i)
                                phi23[i] = c_phi23[i] + m[i];
                }
        }
}

void Sys3D::n_assemble_Opt(const Body3D& parent, 
                           const DCAAssembly3D& child,
                           DCAAssembly3D& A,
                           const bool& calcH2,
                           const bool& calcH1){
        A.parentid_ = parent.id_;
        A.childid_ = child.childid_;
        A.udotid_ = child.parentid_;
        A.parentasmid_ = -1;        
        A.childasmid_ = child.asmid_;
        
        const Body3D& parentbody = parent;
        const Body3D& childbody = barray_[parent.id_+1];
        
        const Vect3& p_w = parentbody.w_;
        const Vect3& c_w = childbody.w_;
        const double *J = parentbody.R_NM_.Ptr();
        
        const DCAData3D& p = parent.dca_;
        const DCAData3D& c = child.dca_;
        
        const double *p_phi11 = p.phi11_.Ptr();
        const double *p_phi12 = p.phi12_.Ptr();
        const double *p_phi13 = p.phi13_.Ptr();
        
        const double *p_phi21 = p.phi21_.Ptr();
        const double *p_phi22 = p.phi22_.Ptr();
        const double *p_phi23 = p.phi23_.Ptr();
        
        const double *c_phi11 = c.phi11_.Ptr();
        const double *c_phi12 = c.phi12_.Ptr();
        const double *c_phi13 = c.phi13_.Ptr();
        
        const double *c_phi21 = c.phi21_.Ptr();
        const double *c_phi22 = c.phi22_.Ptr();
        const double *c_phi23 = c.phi23_.Ptr();
        
        // t(0,35) = D*inv{(~D)*(c.phi11_ + p.phi22_)*D}*(~D)
        double T[36],z[25],t[36];
        for(int i=0;i<36;++i)
                T[i]=c_phi11[i]+p_phi22[i];
        
        calc_xtilde(T,J,z);
        calc_DXDT(z,J,t);
        
        double *wa = A.Wa_.wPtr();                 
        double *wb = A.Wb_.wPtr();                 
        double *wc = A.Wc_.wPtr();      
        
        if(calcH1){
                // calculate Wa
                // A.Wa_ = X*p.phi21_;                         
                FastOps::mult_mat66_mat66(t,p_phi21,wa);
        }
        
        if(calcH2){
                // calculate Wb
                // A.Wb_ = -X*c.phi12_;                                
                FastOps::mmult_mat66_mat66(t,c_phi12,wb);
        }        
        {
                double w[3],z[6];
                FastOps::cross(p_w.Ptr(),c_w.Ptr(),w);
                // calculate Wc
                // A.Wc_ = Vect6(X*(p.phi23_ - c.phi13_+ dot(H)u));        
                for(int i=0;i<6;++i)
                        z[i]=p_phi23[i]-c_phi13[i];
                z[0]+=w[0];
                z[1]+=w[1];
                z[2]+=w[2];
                FastOps::mult_mat66_vec6(t,z,wc);                
        }
        double m[36];
        if(calcH1){
                // calculate A.dca_.phi11_
                // A.dca_.phi11_ = p.phi11_ - p.phi12_*A.Wa_;
                double *phi11 = A.dca_.phi11_.wPtr();                
                FastOps::mult_mat66_mat66(p_phi12,wa,m);
                for(int i=0;i<36;++i)
                        phi11[i] = p_phi11[i]-m[i];        
                
                if(calcH2){
                        // calculate A.dca_.phi12_
                        // A.dca_.phi12_ = -p.phi12_*A.Wb_;
                        double *phi12 = A.dca_.phi12_.wPtr();                
                        FastOps::mmult_mat66_mat66(p_phi12,wb,phi12);                
                }
                
                {
                        // calculate A.dca_.phi13_
                        // A.dca_.phi13_ = p.phi13_ - p.phi12_*A.Wc_
                        double *phi13 = A.dca_.phi13_.wPtr();
                        FastOps::mult_mat66_vec6(p_phi12,wc,m);
                        for(int i=0;i<6;++i)
                                phi13[i] = p_phi13[i]-m[i];                
                }
        }
        
        if(calcH2){
                if(calcH1){
                        // calculate A.dca_.phi21_
                        // A.dca_.phi21_ = c.phi21_*A.Wa_;
                        double *phi21 = A.dca_.phi21_.wPtr();
                        FastOps::mult_mat66_mat66(c_phi21,wa,phi21);
                }
                {
                        // calculate A.dca_.phi22_
                        // A.dca_.phi22_ = c.phi22_ + c.phi21_*A.Wb_;
                        double *phi22 = A.dca_.phi22_.wPtr();
                        FastOps::mult_mat66_mat66(c_phi21,wb,m);
                        for(int i=0;i<36;++i)
                                phi22[i]=c_phi22[i]+m[i];                
                }
                {
                        // calculate A.dca_.phi23_
                        // calculate A.dca_.phi23_ = c.phi23_ + c.phi21_*A.Wc_;
                        double *phi23 = A.dca_.phi23_.wPtr();
                        FastOps::mult_mat66_vec6(c_phi21,wc,m);
                        for(int i=0;i<6;++i)
                                phi23[i] = c_phi23[i] + m[i];
                }
        }
}

void Sys3D::initDCANodesForDCA(){
        if(!Asm_){           
                #ifdef RSIM_SAFE        
                // number of processors greater than 0
                assert(nproc_>0);
                assert(nproc_<=nbodies_);
                double lproc = log2(nproc_);
                
                // number of processors should be a power of 2 or equal to 1
                assert(!(lproc - floor(lproc)) || nproc_==1);
                
                lproc = nbodies_/nproc_;        
                // number of bodies should be divisible by the number of processors
                assert(!(lproc - floor(lproc)));
                #endif               
                
                nassem_ = nbodies_-1;
                Asm_ = new DCAAssembly3D[nassem_];
                for(int i=0;i<nassem_;++i)
                        Asm_[i].asmid_ = i;
        }
}

void Sys3D::setQ(const double& q){
        
        for(int i=0;i<nbodies_;++i){
                barray_[i].q_ = q;         
        }
}

void Sys3D::setQ(){
        srand ( time(NULL) );
        
        for(int i=0;i<nbodies_;++i){
                barray_[i].q_ = i;         
        }
}

void Sys3D::setU(const double& u){
        for(int i=0;i<nbodies_;++i){
                barray_[i].u_ = u;         
        }
}

void Sys3D::initDCANodesForDCAABA(){
        if(!Asm_){
                #ifdef RSIM_SAFE
                
                // number of processors greater than 0
                assert(nproc_>0);
                assert(nproc_<nbodies_ || nbodies_ == 1);
                double lproc = log2(nproc_);
                
                // number of processors should be a power of 2 or equal to 1
                assert(!(lproc - floor(lproc)) || nproc_==1);
                
                lproc = nbodies_/nproc_;        
                // number of bodies should be divisible by the number of processors
                assert(!(lproc - floor(lproc)));
                #endif               
                
                nassem_ = 2*nproc_-1;
                Asm_ = new DCAAssembly3D[nassem_];
                for(int i=0;i<nassem_;++i)
                        Asm_[i].asmid_ = i;
        }
}

void Sys3D::calcNullSpaceProjectionMatrix(){
        
        for(int i=0;i<nbodies_;++i)
                barray_[i].calcNullSpaceProjectionMatrixRow(nbodies_,
                                                         pinvjac_.Ptr(),
                                                         null_.wPtr());
}

void Sys3D::calcNullSpaceProjectionMatrix(const int& start_id,
                                          const int& end_id){        
        
        
        for(int i=start_id;i<=end_id;++i)
                barray_[i].calcNullSpaceProjectionMatrixRow(nbodies_,
                                                            pinvjac_.Ptr(),
                                                            null_.wPtr());
                                        
}

void Sys3D::n_triangularizeBackward(const int& root,const int& leaf){
        #ifdef RSIM_SAFE
        assert(leaf >= root);
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif
        if(leaf){
                barray_[leaf].n_triangularizeBackward(barray_+leaf-1,NULL);                
                for(int i=leaf-1;i>root;--i){
                        barray_[i].n_triangularizeBackward(barray_+i-1,barray_+i+1);
                }
                barray_[root].n_triangularizeBackward(NULL,barray_+root+1);
        }
        else{
                // This is the case when leaf = root = 0
                Body3D& body = barray_[0];                
                double *I = body.aba_.Ik3_.wPtr();
                double *f = body.aba_.Fk3_.wPtr();        
        
                body.setIandF(I,f);
        }
}

void Sys3D::n_triangularizeForward(const int& root,
                                   const int& leaf){
        #ifdef RSIM_SAFE
        assert(leaf > root);
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif
}

void Sys3D::n_substituteForward( const int& root,
                                 const int& leaf,
                                 Body3D *parent){
        #ifdef RSIM_SAFE
        assert(leaf >= root);
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif
        
        barray_[root].n_substituteForward(parent);
        for(int i=root+1;i<=leaf;++i)
                barray_[i].n_substituteForward(barray_+i-1);        
}

void Sys3D::solveSystemHandleEquationsCaseI_Opt(){        
                
        const DCAData3D& dca = Asm_[nassem_-1].dca_;
        Body3D& parent = barray_[0];
          
        const double *phi11 = dca.phi11_.Ptr();
        const double *phi13 = dca.phi13_.Ptr();
        double *fh1 = parent.eom_.fh1_.wPtr();                        
        Vector R(5);
        double t[25];
        double *r = R.wPtr();
        
        // t = D' * phi11 * D
        t[0]=phi11[0];  t[1]=phi11[1];  t[2]=phi11[3];  t[3]=phi11[4];  t[4]=phi11[5];
        t[5]=phi11[6];  t[6]=phi11[7];  t[7]=phi11[9];  t[8]=phi11[10];  t[9]=phi11[11];
        t[10]=phi11[18];  t[11]=phi11[19];  t[12]=phi11[21];  t[13]=phi11[22];  t[14]=phi11[23];
        t[15]=phi11[24];  t[16]=phi11[25];  t[17]=phi11[27];  t[18]=phi11[28];  t[19]=phi11[29];
        t[20]=phi11[30];  t[21]=phi11[31];  t[22]=phi11[33];  t[23]=phi11[34];  t[24]=phi11[35];
        
        // r = D'*phi13
        r[0]=phi13[0];r[1]=phi13[1];r[2]=phi13[3];r[3]=phi13[4];r[4]=phi13[5];                        
        
        Cholesky chol(Matrix(5,5,t));
        Vector F = chol.solve(-R);        
        fh1[0]= F.Ptr()[0];
        fh1[1]= F.Ptr()[1];
        fh1[2]= 0.0;
        fh1[3]= F.Ptr()[2];
        fh1[4]= F.Ptr()[3];
        fh1[5]= F.Ptr()[4];
        parent.eom_.ah1_ = dca.phi11_*parent.eom_.fh1_ + dca.phi13_;
        parent.udot_ = parent.eom_.ah1_[2];
}

// This is not the world's best implementation, but this set of operation is common for 
// both serial and parallel algorithms and hence perhaps we can get away with it.
void Sys3D::solveSystemHandleEquationsCase_OpControl(){
                
        Matrix D(6,5);
        double *d = D.wPtr();
        for(int i=0;i<30;++i)
                d[i]=0.0;
        d[0] = d[6] = d[17] = d[23] = d[29] = 1.0;

        const DCAData3D& dca = Asm_[nassem_-1].dca_;
        
        const double *phi11 = dca.phi11_.Ptr();
        const double *phi13 = dca.phi13_.Ptr();        
        Vector R(5);
        double t[25];
        double *r = R.wPtr();
        
        // t = D' * phi11 * D
        t[0]=phi11[0];  t[1]=phi11[1];  t[2]=phi11[3];  t[3]=phi11[4];  t[4]=phi11[5];
        t[5]=phi11[6];  t[6]=phi11[7];  t[7]=phi11[9];  t[8]=phi11[10];  t[9]=phi11[11];
        t[10]=phi11[18];  t[11]=phi11[19];  t[12]=phi11[21];  t[13]=phi11[22];  t[14]=phi11[23];
        t[15]=phi11[24];  t[16]=phi11[25];  t[17]=phi11[27];  t[18]=phi11[28];  t[19]=phi11[29];
        t[20]=phi11[30];  t[21]=phi11[31];  t[22]=phi11[33];  t[23]=phi11[34];  t[24]=phi11[35];
                        
        // r = D'*phi13
        r[0]=phi13[0];r[1]=phi13[1];r[2]=phi13[3];r[3]=phi13[4];r[4]=phi13[5];                        
                
        Cholesky chol(Matrix(5,5,t));                        
        
        // M1 = -D' * phi12
        Matrix M1 = -(~D)*dca.phi12_;                
        Matrix M2;
        
        // M2 = inv(t)*M1
        chol.solve(M1,M2);        
        
        // M3 = -D*inv(t)*D'*phi12
        Matrix M3 = D*M2;
        
        // F = -inv(t)*D'*phi13
        Vector F = chol.solve(-R);
        
        // M4 = -D*inv(t)*D'*phi13
        Matrix M4 = D*F;
        
        // F1 = M3*F2 + M4
        // substitute in A2 = phi21*F1 + phi22*F2 + phi23 to obtain
        // A2 = iLambda_ * Fhh2 + p_e
        iLambda_ = dca.phi22_ + dca.phi21_*M3;                
        pe_ = dca.phi23_ + dca.phi21_*M4;        
        
        cout<<iLambda_<<endl;
        cout<<pe_<<endl;
        
        /*        
        Body3D& parent = barray_[0];
        Body3D& child = barray_[nbodies_-1];                
                
        Mat6x6 id;
        id.Identity();        
        LU lu(iLambda_);                
        Lambda_ = lu.solve(id);         
        
        // Part below is required only for calculating inertia weighted pseudoinverse
        for(int i=0;i<6;++i){
                double *fh1 = parent.eom_.Fh1_[i].wPtr();
                double *fh2 = child.eom_.Fh2_[i].wPtr();        
                for(int j=0;j<6;++j){
                        // fh1 = ilambda * H(i)
                        // H(i) is 6 x 1 with just i^th entry as 1
                        fh2[j] = Lambda_(j,i);                                               
                }
                FastOps::mult_mat66_vec6(M3.Ptr(),fh2,fh1);
                parent.eom_.Ah1_[i] = dca.phi11_*parent.eom_.Fh1_[i]+
                                      dca.phi12_*child.eom_.Fh2_[i];  
                
                pinvjac_(parent.id_,i) = parent.eom_.Ah1_[i][2];
        }        */
}

void Sys3D::solveSystemHandleEquationsCaseII_Opt(){        
        
        DCAData3D& dca = Asm_[nassem_-1].dca_;
        Body3D& parent = barray_[0];        
        Body3D& child = barray_[nbodies_-1];
        double *fh2 = child.eom_.fh2_.wPtr();
        for(int i=0;i<3;fh2[i++]=100.0);
        
        const double *phi11 = dca.phi11_.Ptr();
        double *phi13 = dca.phi13_.wPtr();
        
        double p[6];
        FastOps::mult_mat63_vec3(dca.phi12_.Ptr(),fh2,p);
        for(int i=0;i<6;++i)
                phi13[i]+=p[i];
                
        
        double *fh1 = parent.eom_.fh1_.wPtr();                                
        
        double t[12];        
        FastOps::inv_mat3x3(phi11+9,t);
        FastOps::mult_mat33_vec3(t,phi13+3,t+9);        
        fh1[0]=-t[9];fh1[1]=-t[10];fh1[2]=-t[11];        
        
        double *ah1 = parent.eom_.ah1_.wPtr();
        FastOps::mult_mat63_vec3(phi11,fh1,t);
        ah1[0] = t[0] + phi13[0];
        ah1[1] = t[1] + phi13[1];
        ah1[2] = t[2] + phi13[2];
        
        ah1[3] = t[3] + phi13[3];
        ah1[4] = t[4] + phi13[4];
        ah1[5] = t[5] + phi13[5];        
}

void Sys3D::disassembleDCAAssembly_Opt( const int& start_id,
                                        const int& end_id,
                                        const bool& calcH2,
                                        const bool& calc_pinvjac){
        for(int i=start_id;i>=end_id;disassembleDCAAssembly_Opt(i--,calcH2,calc_pinvjac));
                
}

void Sys3D::disassembleDCAAssembly_Opt(const int& id,
                                       const bool& calcH2,
                                       const bool& calc_pinvjac){
        
        DCAAssembly3D& ASM = Asm_[id];                
        
        Body3D& asm_parent = barray_[ASM.parentid_];
        Body3D& asm_child = barray_[ASM.childid_];
        Body3D& udot_parent = barray_[ASM.udotid_-1];
        Body3D& udot_child = barray_[ASM.udotid_];                
        if(!calc_pinvjac){
                double t[12];
                {
                        double *cfh1 = udot_child.eom_.fh1_.wPtr();
                        double *pfh2 = udot_parent.eom_.fh2_.wPtr();
                        const double *wc = ASM.Wc_.Ptr();
                        FastOps::mult_mat66_vec6(ASM.Wa_.Ptr(),asm_parent.eom_.fh1_.Ptr(),t);
                        cfh1[0]=t[0]+wc[0];
                        cfh1[1]=t[1]+wc[1];
                        cfh1[2]=t[2]+wc[2];
                        cfh1[3]=t[3]+wc[3];
                        cfh1[4]=t[4]+wc[4];
                        cfh1[5]=t[5]+wc[5];
                        if(calcH2){
                                FastOps::mult_mat66_vec6(ASM.Wb_.Ptr(),asm_child.eom_.fh2_.Ptr(),t);
                                cfh1[0] += t[0];
                                cfh1[1] += t[1];
                                cfh1[2] += t[2];
                                cfh1[3] += t[3];
                                cfh1[4] += t[4];
                                cfh1[5] += t[5];
                        }

                        pfh2[0] = -cfh1[0];
                        pfh2[1] = -cfh1[1];
                        pfh2[2] = -cfh1[2];
                        pfh2[3] = -cfh1[3];
                        pfh2[4] = -cfh1[4];
                        pfh2[5] = -cfh1[5];                
                }
                //////////////////////////////////////////////
                // This part was to take care of hte fact that hte 
                // assembled system could have bodies as a parent or child
                DCAData3D *ASM_parent_ptr, *ASM_child_ptr;        
                if(ASM.parentasmid_==-1)
                        ASM_parent_ptr = &(asm_parent.dca_);
                else
                        ASM_parent_ptr = &(Asm_[ASM.parentasmid_].dca_);
                
                if(ASM.childasmid_==-1)
                        ASM_child_ptr = &(asm_child.dca_);
                else
                        ASM_child_ptr = &(Asm_[ASM.childasmid_].dca_);
                //////////////////////////////////////////////
                                
                {
                        double *pah2 = udot_parent.eom_.ah2_.wPtr();
                        const double *p23 = ASM_parent_ptr->phi23_.Ptr();
                        FastOps::mult_mat66_vec6(ASM_parent_ptr->phi21_.Ptr(),asm_parent.eom_.fh1_.Ptr(),t);
                        FastOps::mult_mat66_vec6(ASM_parent_ptr->phi22_.Ptr(),udot_parent.eom_.fh2_.Ptr(),t+6);
                        for(int i=0;i<6;++i)
                                pah2[i]=t[i]+t[i+6]+p23[i];                
                
                        double *cah1 = udot_child.eom_.ah1_.wPtr();
                        const double *p13 = ASM_child_ptr->phi13_.Ptr();
                        FastOps::mult_mat66_vec6(ASM_child_ptr->phi11_.Ptr(),udot_child.eom_.fh1_.Ptr(),t);
                        for(int i=0;i<6;++i)
                                cah1[i]=t[i]+p13[i];                
                        
                        if(calcH2){
                                FastOps::mult_mat66_vec6(ASM_child_ptr->phi12_.Ptr(),asm_child.eom_.fh2_.Ptr(),t+6);                
                                for(int i=0;i<6;++i)
                                        cah1[i] += t[i+6];                        
                        }                              
                        FastOps::cross(udot_parent.w_.Ptr(),udot_child.w_.Ptr(),t);
                        t[3] = cah1[0] - pah2[0] - t[0];
                        t[4] = cah1[1] - pah2[1] - t[1];
                        t[5] = cah1[2] - pah2[2] - t[2];

                        FastOps::transpose_mult_mat33_vec3(udot_parent.R_NM_.Ptr(),t+3,t);
                        udot_child.udot_ = t[2];
                }
        }//if(!calc_pinvjac)
        else{                                
                for(int loop=0;loop<6;++loop){
                        double t[12];
                        {
                                double *cfh1 = udot_child.eom_.Fh1_[loop].wPtr();                                
                                double *pfh2 = udot_parent.eom_.Fh2_[loop].wPtr();                                
                                
                                FastOps::mult_mat66_vec6(ASM.Wa_.Ptr(),asm_parent.eom_.Fh1_[loop].Ptr(),t);
                                cfh1[0]=t[0];
                                cfh1[1]=t[1];
                                cfh1[2]=t[2];
                                cfh1[3]=t[3];
                                cfh1[4]=t[4];
                                cfh1[5]=t[5];
                                
                                FastOps::mult_mat66_vec6(ASM.Wb_.Ptr(),asm_child.eom_.Fh2_[loop].Ptr(),t);
                                cfh1[0] += t[0];
                                cfh1[1] += t[1];
                                cfh1[2] += t[2];
                                cfh1[3] += t[3];
                                cfh1[4] += t[4];
                                cfh1[5] += t[5];                                

                                pfh2[0] = -cfh1[0];
                                pfh2[1] = -cfh1[1];
                                pfh2[2] = -cfh1[2];
                                pfh2[3] = -cfh1[3];
                                pfh2[4] = -cfh1[4];
                                pfh2[5] = -cfh1[5];                
                        }
                        //////////////////////////////////////////////
                        // This part was to take care of hte fact that hte 
                        // assembled system could have bodies as a parent or child
                        DCAData3D *ASM_parent_ptr, *ASM_child_ptr;        
                        if(ASM.parentasmid_==-1)
                                ASM_parent_ptr = &(asm_parent.dca_);
                        else
                                ASM_parent_ptr = &(Asm_[ASM.parentasmid_].dca_);
                        
                        if(ASM.childasmid_==-1)
                                ASM_child_ptr = &(asm_child.dca_);
                        else
                                ASM_child_ptr = &(Asm_[ASM.childasmid_].dca_);
                        //////////////////////////////////////////////
                                        
                        {
                                double *pah2 = udot_parent.eom_.Ah2_[loop].wPtr();                                
                                FastOps::mult_mat66_vec6(ASM_parent_ptr->phi21_.Ptr(),asm_parent.eom_.Fh1_[loop].Ptr(),t);
                                FastOps::mult_mat66_vec6(ASM_parent_ptr->phi22_.Ptr(),udot_parent.eom_.Fh2_[loop].Ptr(),t+6);
                                for(int i=0;i<6;++i)
                                        pah2[i]=t[i]+t[i+6];
                        
                                double *cah1 = udot_child.eom_.Ah1_[loop].wPtr();                                
                                FastOps::mult_mat66_vec6(ASM_child_ptr->phi11_.Ptr(),udot_child.eom_.Fh1_[loop].Ptr(),t);
                                for(int i=0;i<6;++i)
                                        cah1[i]=t[i];
                                                                
                                FastOps::mult_mat66_vec6(ASM_child_ptr->phi12_.Ptr(),asm_child.eom_.Fh2_[loop].Ptr(),t+6);                
                                for(int i=0;i<6;++i)
                                        cah1[i] += t[i+6];                        
                                
                                // FastOps::cross(udot_parent.w_.Ptr(),udot_child.w_.Ptr(),t);
                                t[3] = cah1[0] - pah2[0];// - t[0];
                                t[4] = cah1[1] - pah2[1];// - t[1];
                                t[5] = cah1[2] - pah2[2];// - t[2];

                                FastOps::transpose_mult_mat33_vec3(udot_parent.R_NM_.Ptr(),t+3,t);
                                pinvjac_(udot_child.id_,loop) = t[2];
                        }
                }                
        }
}

void Sys3D::disassembleABAAssembly_Opt(const int& id,
                                       const bool& calcH2,
                                       const bool& calc_jacpinv){
        const int& pid = Asm_[id].parentid_;
        const int& cid = Asm_[id].childid_;
        if(!calc_jacpinv){
                if(calcH2){                
                        const Vect6& fh2 = barray_[cid].eom_.fh2_;
                        
                        for(int i=pid+1;i<=cid;++i){
                                Body3D& bod = barray_[i];                        
                                const Body3D& parent = barray_[i-1];
                                const double *J = parent.R_NM_.Ptr();
                                const double *p_ah1 = parent.eom_.ah1_.Ptr();
                        
                                double t[2];
                                FastOps::dot6(bod.m1_.Ptr(),p_ah1,t);
                                FastOps::dot6(bod.m3_.Ptr(),fh2.Ptr(),t+1);
                                bod.udot_ = t[0]+t[1]+bod.m2_;
                                
                                double z[6];
                                FastOps::transpose_skbeta_x_vec6(parent.nbetak_.Ptr(),p_ah1,z);
                                double *ah1 = bod.eom_.ah1_.wPtr();
                                const double *akt = bod.Akt_.Ptr();
                                
                                ah1[0] = z[0]+akt[0]+J[2]*bod.udot_;
                                ah1[1] = z[1]+akt[1]+J[5]*bod.udot_;
                                ah1[2] = z[2]+akt[2]+J[8]*bod.udot_;
                                ah1[3] = z[3]+akt[3];
                                ah1[4] = z[4]+akt[4];
                                ah1[5] = z[5]+akt[5];                        
                        }                
                }
                else{                   
                        n_substituteForward(pid+1,cid,barray_+pid);                        
                }
        }
        else{
                for(int n=0;n<6;++n){
                        const Vect6& fh2 = barray_[cid].eom_.Fh2_[n];                        
                        
                        for(int i=pid+1;i<=cid;++i){
                                Body3D& bod = barray_[i];                        
                                const Body3D& parent = barray_[i-1];
                                const double *J = parent.R_NM_.Ptr();
                                const double *p_ah1 = parent.eom_.Ah1_[n].Ptr();
                        
                                double t[2];
                                FastOps::dot6(bod.m1_.Ptr(),p_ah1,t);
                                FastOps::dot6(bod.m3_.Ptr(),fh2.Ptr(),t+1);
                                pinvjac_(bod.id_,n) = t[0]+t[1];
                                
                                double z[6];
                                FastOps::transpose_skbeta_x_vec6(parent.nbetak_.Ptr(),p_ah1,z);
                                double *ah1 = bod.eom_.Ah1_[n].wPtr();                                
                                
                                ah1[0] = z[0]+J[2]*pinvjac_(bod.id_,n);
                                ah1[1] = z[1]+J[5]*pinvjac_(bod.id_,n);
                                ah1[2] = z[2]+J[8]*pinvjac_(bod.id_,n);
                                ah1[3] = z[3];
                                ah1[4] = z[4];
                                ah1[5] = z[5];                        
                        }
                }
        }
        
}

void Sys3D::n_calcDCAHandleEquationsUsingABA_Opt(const int& root,
                                                 const int& leaf,
                                                 const int& nodeid,
                                                 const bool& calcH2,
                                                 const bool& nbody){
        #ifdef RSIM_SAFE
        assert(leaf >= root);
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif
        
        if(nbody){
                n_triangularizeBackward(root,leaf);
                
                if(calcH2){
                        n_calcZeta12Backward(root,leaf);        
                        n_calcZetaForward_Opt(root,leaf,NULL);                                
                        barray_[leaf].n_shiftPhiToOutboardHandle_Opt(barray_+root);
                }
                else
                        barray_[root].n_calcZetaForward_Opt(root+1,NULL,calcH2);
                
                copyHandleEquationsIntoAssembly(root,leaf,nodeid);
        }
        else{
                n_triangularizeBackward(root,leaf);
                n_calcZeta12Backward(root,leaf);        
                n_calcZetaForward_Opt(root,leaf,NULL);                                
                barray_[leaf].n_shiftPhiToOutboardHandle_Opt(barray_+root,false);
                copyHandleEquationsIntoAssembly(root,leaf,nodeid);                
        }
        
}

void Sys3D::n_calcZetaForward_Opt(const int& root,
                                  const int& leaf,
                                  const Body3D *parent){
        #ifdef RSIM_SAFE
        assert(leaf >= root);
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif
        const int& rootP1 = root + 1;
        barray_[root].n_calcZetaForward_Opt(rootP1, parent);
        for(int i=root+1;i<=leaf;++i)
                barray_[i].n_calcZetaForward_Opt(rootP1, barray_+i-1);
}

void Sys3D::n_calcZeta12Backward(const int& root,const int& leaf){
        #ifdef RSIM_SAFE
        assert(leaf >= root);
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif
        
        barray_[leaf].n_calcZeta12Backward();
        for(int i=leaf-1;i>=root;--i)
                barray_[i].n_calcZeta12Backward(barray_+i+1);
}

void Sys3D::updRotationMatrix(){
        barray_[0].updRotationMatrix();
        barray_[0].updvon();
        for(int i=1;i<nbodies_;++i){                
                barray_[i].updRotationMatrix(barray_+i-1);
                barray_[i].updvon(barray_[i-1]);
        }
}

void Sys3D::assembleKinematicSweep(const int& root,
                                   const int& leaf){
        #ifdef RSIM_SAFE
        assert(leaf > root);
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif
        
        // Update rotation Matrix
        barray_[root].updRotationMatrix(true);        
        for(int i=root+1;i<=leaf;++i){
                barray_[i].updRotationMatrix(barray_+i-1,true);
                barray_[i].updcbetak(barray_+i-1);                
        }        
        
        // Update Angular and Linear Velocity
        barray_[root].updAngularVelocity();
        for(int i=root+1;i<=leaf;++i)
                barray_[i].updAngularVelocity(barray_+i-1,false);        
}

void Sys3D::setKinematicBoundaryConditions(const int *ids,const int& nproc){
        // set boundary rotation matrices
        for(int i=2;i<=nproc;++i){
                const Body3D& parent = barray_[ids[i-1]-1];
                Body3D& child = barray_[ids[i]-1];
                child.updRotationMatrix(parent.R_NM_);
                child.updcbetak(parent.R_NM_);                
                child.updAngularVelocity(parent);                                
        }
}

void Sys3D::setKinematicBoundaryConditions(){
        // set boundary rotation matrices
        for(int i=2;i<=nproc_;++i){
                const Body3D& parent = barray_[ids_[i-1]-1];
                Body3D& child = barray_[ids_[i]-1];
                child.updRotationMatrix(parent.R_NM_);
                child.updcbetak(parent.R_NM_);                                
                child.updAngularVelocity(parent);         
                
                const double *pvon = parent.v_on_.Ptr();
                const double *beta = parent.nbetak_.Ptr();
                double *cvon = child.v_on_.wPtr();
                cvon[0] += pvon[0] + beta[0];
                cvon[1] += pvon[1] + beta[1];
                cvon[2] += pvon[2] + beta[2];
        }      
        double *on = ON_.wPtr();
        const double *von = barray_[nbodies_-1].v_on_.Ptr();
        on[0]=von[0];   
        on[1]=von[1];
        on[2]=von[2];
}

void Sys3D::disassembleKinematicSweep(  const int& root,
                                        const int& leaf){
        #ifdef RSIM_SAFE
        assert(leaf > root);
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif
        const double *on = ON_.Ptr();
        if(root){
                const Body3D& parent = barray_[root-1];                
                const double *pv = parent.v_.Ptr();
                double t[3];
                FastOps::cross( parent.w_.Ptr(),
                                parent.nbetak_.Ptr(),
                                t);
                t[0] += pv[0];
                t[1] += pv[1];
                t[2] += pv[2];
                
                barray_[root].updRotationMatrix(parent.R_NM_);
                barray_[root].updAngularAndLinearVelocity(parent,NULL,t);
                barray_[root].updvon(barray_[root-1]);
                barray_[root].calcvin(on);
                barray_[root].calcJFromvin();
                for(int i=root+1;i<leaf;++i){
                        barray_[i].updRotationMatrix(parent.R_NM_);
                        barray_[i].updcbetak(parent.R_NM_);
                        barray_[i].updvon(barray_[i-1]);
                        barray_[i].calcvin(on);
                        barray_[i].calcJFromvin();
                        barray_[i].updAngularAndLinearVelocity(parent,barray_+i-1,t);
                }
                barray_[leaf].calcvin(on);
                barray_[leaf].calcJFromvin();
        }
        else{
                for(int i=root;i<=leaf;++i){
                        barray_[i].calcvin(on);
                        barray_[i].calcJFromvin();
                }
        }
}

void Sys3D::updAngularVelocity(){
        barray_[0].updAngularVelocity();
        for(int i=1;i<nbodies_;++i){
                barray_[i].updAngularVelocity(barray_+i-1);
        }
}

void Sys3D::calcJ(){
        barray_[nbodies_-1].calcJ();
        for(int i=nbodies_-2;i>=0;--i){
               barray_[i].calcJ(barray_+i+1);
        }
}

void Sys3D::calcJTx(const double *x){
        barray_[nbodies_-1].calcJTx(x);
        double *v = barray_[nbodies_-1].v_in_.wPtr();
        v[0] = 0.0;v[1] = 0.0;v[2] = 0.0;
        for(int i=nbodies_-2;i>=0;--i){
               barray_[i].calcJTx(barray_+i+1,x);
        }
        
}

void Sys3D::n_formInboardEOM(){        
        for(int i=0;i<nbodies_;++i){
                barray_[i].n_formInboardEOM(Gravity_);
        }
}

void Sys3D::n_formInboardEOM(const int& root,
                             const int& leaf){                
        #ifdef RSIM_SAFE
        assert(leaf > root || (leaf == root && leaf == 0));
        assert(root>=0);
        assert(leaf<nbodies_);
        #endif
        
        if(root){                
                barray_[root].n_formInboardEOM(barray_+root-1,Gravity_);                        
        }
        else{
                barray_[root].n_formInboardEOM(Gravity_);                
        }
        for(int i=root+1;i<=leaf;++i){
                barray_[i].n_formInboardEOM(barray_+i-1,Gravity_);
        }
}

void Sys3D::displayQ(){
        for(int i=0;i<nbodies_;++i){                
                cout<<barray_[i].q_<<"\n";
        }
}

void Sys3D::displayU(){
        for(int i=0;i<nbodies_;++i){                
                cout<<barray_[i].u_<<"\n";
        }
}

void Sys3D::displayUDot(){
        for(int i=0;i<nbodies_;++i){                
                cout<<barray_[i].udot_<<"\n";
        }
}

void Sys3D::displayUDot1(){
        cout<<setprecision(16);
        for(int i=0;i<nbodies_;++i){                
                cout<<barray_[i].udot_<<"\n";
        }
}

void Sys3D::displayAngularVelocity(){
        cout<<setprecision(16);
        for(int i=0;i<nbodies_;++i)
                cout<<barray_[i].w_<<"\n";
}

void Sys3D::displayLinearVelocity(){
        cout<<setprecision(16);
        for(int i=0;i<nbodies_;++i)
                cout<<barray_[i].v_[0]<<"\t"<<barray_[i].v_[1]<<"\t"<<barray_[i].v_[2]<<"\n";
}

void Sys3D::displayIds(){        
        for(int i=0;i<nproc_+1;++i){                
                cout<<ids_[i]<<"\n";
        }
}

void Sys3D::displayTau(){
        cout<<setprecision(16);
        for(int i=0;i<nbodies_;++i)
                cout<<barray_[i].tau_<<endl;
}

void Sys3D::displayPosition(){
        cout<<setprecision(16);
        for(int i=0;i<nbodies_;++i)
                cout<<barray_[i].v_on_[0]<<"\t"<<barray_[i].v_on_[1]<<"\t"<<barray_[i].v_on_[2]<<"\n";
}

void Sys3D::displayvin(){
        cout<<setprecision(16);
        for(int i=0;i<nbodies_;++i)
                cout<<barray_[i].v_in_[0]<<"\t"<<barray_[i].v_in_[1]<<"\t"<<barray_[i].v_in_[2]<<"\n";
}


void Sys3D::displayJac(){
        cout<<setprecision(16);
        for(int i=0;i<nbodies_;++i){
                const double *jac = barray_[i].jac_.Ptr();
                for(int j=0;j<6;++j)
                        cout<<jac[j]<<"\t";
                cout<<endl;
        }
}